#include <rtdevice.h>
#include <board.h>

#define DCMI_TIM_PCLK_FREQ()    HAL_RCC_GetPCLK2Freq()
#define DCMI_TIM_CHANNEL        (TIM_CHANNEL_1)
#define DCMI_TIM                (TIM1)

#define IRQ_PRI_DCMI NVIC_EncodePriority(NVIC_PRIORITYGROUP_4, 3, 0)
#define I2C_TIMING_STANDARD     (0x20D09DE7)

#define DCMI_RESET_PIN          (GPIO_PIN_10)
#define DCMI_RESET_PORT         (GPIOA)
#define DCMI_PWDN_PIN           (GPIO_PIN_7)
#define DCMI_PWDN_PORT          (GPIOD)

#define DCMI_RESET_LOW()        HAL_GPIO_WritePin(DCMI_RESET_PORT, DCMI_RESET_PIN, GPIO_PIN_RESET)
#define DCMI_RESET_HIGH()       HAL_GPIO_WritePin(DCMI_RESET_PORT, DCMI_RESET_PIN, GPIO_PIN_SET)
#define DCMI_PWDN_LOW()         HAL_GPIO_WritePin(DCMI_PWDN_PORT, DCMI_PWDN_PIN, GPIO_PIN_RESET)
#define DCMI_PWDN_HIGH()        HAL_GPIO_WritePin(DCMI_PWDN_PORT, DCMI_PWDN_PIN, GPIO_PIN_SET)

#define I2C_TIMEOUT 1000

struct d_camif
{
    rt_camif_t ci;
    DCMI_HandleTypeDef DCMIHandle;
    DMA_HandleTypeDef DMAHandle;
    I2C_HandleTypeDef sccb;
};

static struct d_camif _d_camif;

void DCMI_IRQHandler(void)
{
    /* enter interrupt */
    rt_interrupt_enter();

    HAL_DCMI_IRQHandler(&_d_camif.DCMIHandle);

    /* leave interrupt */
    rt_interrupt_leave();
}

void DMA2_Stream1_IRQHandler(void) 
{
    rt_interrupt_enter();

    HAL_DMA_IRQHandler(&_d_camif.DMAHandle);

    rt_interrupt_leave();
}

void HAL_DCMI_FrameEventCallback(DCMI_HandleTypeDef *hdcmi)
{

}

void DCMI_DMAConvCpltUser(uint32_t addr)
{

}

static TIM_HandleTypeDef  TIMHandle  = {0};

static int extclk_config(int frequency)
{
    /* TCLK (PCLK * 2) */
    int tclk = DCMI_TIM_PCLK_FREQ() * 2;

    /* Period should be even */
    int period = (tclk / frequency) - 1;

    if (TIMHandle.Init.Period && (TIMHandle.Init.Period != period))
    {
        // __HAL_TIM_SET_AUTORELOAD sets TIMHandle.Init.Period...
        __HAL_TIM_SET_AUTORELOAD(&TIMHandle, period);
        __HAL_TIM_SET_COMPARE(&TIMHandle, DCMI_TIM_CHANNEL, period / 2);
        return 0;
    }

    /* Timer base configuration */
    TIMHandle.Instance = DCMI_TIM;
    TIMHandle.Init.Period = period;
    TIMHandle.Init.Prescaler = TIM_ETRPRESCALER_DIV1;
    TIMHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
    TIMHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

    /* Timer channel configuration */
    TIM_OC_InitTypeDef TIMOCHandle;
    TIMOCHandle.Pulse = period / 2;
    TIMOCHandle.OCMode = TIM_OCMODE_PWM1;
    TIMOCHandle.OCPolarity = TIM_OCPOLARITY_HIGH;
    TIMOCHandle.OCFastMode = TIM_OCFAST_DISABLE;
    TIMOCHandle.OCIdleState = TIM_OCIDLESTATE_RESET;
    TIMOCHandle.OCNIdleState = TIM_OCIDLESTATE_RESET;

    if ((HAL_TIM_PWM_Init(&TIMHandle) != HAL_OK) || (HAL_TIM_PWM_ConfigChannel(&TIMHandle, &TIMOCHandle, DCMI_TIM_CHANNEL) != HAL_OK) || (HAL_TIM_PWM_Start(&TIMHandle, DCMI_TIM_CHANNEL) != HAL_OK))
    {
        return -1;
    }

    return 0;
}

static int dma_config(struct d_camif *dcmi)
{
    // DMA Stream configuration
    dcmi->DMAHandle.Instance                  = DMA2_Stream1;             /* Select the DMA instance          */
    #if defined(SOC_SERIES_STM32H7)
    dcmi->DMAHandle.Init.Request              = DMA_REQUEST_DCMI;         /* DMA Channel                      */
    #else
    dcmi->DMAHandle.Init.Channel              = DMA_CHANNEL_1;            /* DMA Channel                      */
    #endif
    dcmi->DMAHandle.Init.Direction            = DMA_PERIPH_TO_MEMORY;     /* Peripheral to memory transfer    */
    dcmi->DMAHandle.Init.MemInc               = DMA_MINC_ENABLE;          /* Memory increment mode Enable     */
    dcmi->DMAHandle.Init.PeriphInc            = DMA_PINC_DISABLE;         /* Peripheral increment mode Enable */
    dcmi->DMAHandle.Init.PeriphDataAlignment  = DMA_PDATAALIGN_WORD;      /* Peripheral data alignment : Word */
    dcmi->DMAHandle.Init.MemDataAlignment     = DMA_MDATAALIGN_WORD;      /* Memory data alignment : Word     */
    dcmi->DMAHandle.Init.Mode                 = DMA_NORMAL;               /* Normal DMA mode                  */
    dcmi->DMAHandle.Init.Priority             = DMA_PRIORITY_HIGH;        /* Priority level : high            */
    dcmi->DMAHandle.Init.FIFOMode             = DMA_FIFOMODE_ENABLE;      /* FIFO mode enabled                */
    dcmi->DMAHandle.Init.FIFOThreshold        = DMA_FIFO_THRESHOLD_FULL;  /* FIFO threshold full              */
    dcmi->DMAHandle.Init.MemBurst             = DMA_PBURST_SINGLE;          /* Memory burst                     */
    dcmi->DMAHandle.Init.PeriphBurst          = DMA_PBURST_SINGLE;        /* Peripheral burst                 */

    // Initialize the DMA stream
    HAL_DMA_DeInit(&dcmi->DMAHandle);
    if (HAL_DMA_Init(&dcmi->DMAHandle) != HAL_OK) {
        // Initialization Error
        return -1;
    }

    // Configure and enable DMA IRQ Channel
    HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);

    return 0;
}

static int dcmi_config(struct d_camif *dcmi, uint32_t jpeg_mode)
{
    DCMI_HandleTypeDef *dh = &dcmi->DCMIHandle;

    // DCMI configuration
    dh->Instance = DCMI;
    // VSYNC clock polarity
    dh->Init.VSPolarity = DCMI_VSPOLARITY_HIGH;
    // HSYNC clock polarity
    dh->Init.HSPolarity = DCMI_HSPOLARITY_HIGH;
    // PXCLK clock polarity
    dh->Init.PCKPolarity = DCMI_PCKPOLARITY_RISING;

    // Setup capture parameters.
    dh->Init.SynchroMode = DCMI_SYNCHRO_HARDWARE;    // Enable Hardware synchronization
    dh->Init.CaptureRate = DCMI_CR_ALL_FRAME;        // Capture rate all frames
    dh->Init.ExtendedDataMode = DCMI_EXTEND_DATA_8B; // Capture 8 bits on every pixel clock
    dh->Init.JPEGMode = jpeg_mode;                   // Set JPEG Mode
#if defined(MCU_SERIES_F7) || defined(SOC_SERIES_STM32H7)
    dh->Init.ByteSelectMode = DCMI_BSM_ALL;   // Capture all received bytes
    dh->Init.ByteSelectStart = DCMI_OEBS_ODD; // Ignored
    dh->Init.LineSelectMode = DCMI_LSM_ALL;   // Capture all received lines
    dh->Init.LineSelectStart = DCMI_OELS_ODD; // Ignored
#endif

    // Associate the DMA handle to the DCMI handle
    __HAL_LINKDMA(dh, DMA_Handle, dcmi->DMAHandle);

    // Initialize the DCMI
    HAL_DCMI_DeInit(dh);
    if (HAL_DCMI_Init(dh) != HAL_OK)
    {
        // Initialization Error
        return -1;
    }

    // Configure and enable DCMI IRQ Channel
    NVIC_SetPriority(DCMI_IRQn, IRQ_PRI_DCMI);
    HAL_NVIC_EnableIRQ(DCMI_IRQn);

    return 0;
}

static int i2c_init(I2C_HandleTypeDef *i2c)
{
    /* Configure I2C */
    i2c->Instance = I2C1;
    i2c->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
#if !defined(STM32F4)
    i2c->Init.Timing = I2C_TIMING_STANDARD;
#else
    i2c->Init.ClockSpeed = I2C_FREQUENCY;
    i2c->Init.DutyCycle = I2C_DUTYCYCLE_2;
#endif
    i2c->Init.DualAddressMode = I2C_DUALADDRESS_DISABLED;
    i2c->Init.GeneralCallMode = I2C_GENERALCALL_DISABLED;
    i2c->Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
    i2c->Init.OwnAddress1 = 0xFE;
    i2c->Init.OwnAddress2 = 0xFE;
    #if !defined(STM32F4)
    i2c->Init.OwnAddress2Masks = 0;
    #endif
    
    HAL_I2C_DeInit(i2c);
    if (HAL_I2C_Init(i2c) != HAL_OK)
    {
        /* Initialization Error */
        return -1;
    }

    return 0;
}

static void dcmi_pdnrst_init(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;
    GPIO_InitStructure.Pull  = GPIO_PULLDOWN;
    GPIO_InitStructure.Speed = GPIO_SPEED_LOW;
    GPIO_InitStructure.Mode  = GPIO_MODE_OUTPUT_PP;

    GPIO_InitStructure.Pin = DCMI_RESET_PIN;
    HAL_GPIO_Init(DCMI_RESET_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.Pin = DCMI_PWDN_PIN;
    HAL_GPIO_Init(DCMI_PWDN_PORT, &GPIO_InitStructure);
}

static int sccb_read8(struct d_camif *dcmi, uint8_t slv, uint8_t reg, uint8_t *d)
{
    if((HAL_I2C_Master_Transmit(&dcmi->sccb, slv, &reg, 1, I2C_TIMEOUT) != HAL_OK) ||
       (HAL_I2C_Master_Receive (&dcmi->sccb, slv, d, 1, I2C_TIMEOUT) != HAL_OK)) 
    {
        return -1;
    }

    return 0;
}

static int sccb_write8(struct d_camif *dcmi, uint8_t slv, uint8_t reg, uint8_t d)
{
    int ret = 0;
    uint8_t buf[] = {reg, d};

    if(HAL_I2C_Master_Transmit(&dcmi->sccb, slv, buf, 2, I2C_TIMEOUT) != HAL_OK) 
    {
        ret = -1;
    }

    return ret;
}

static int d_dcmi_init(rt_camif_t *ci)
{
    struct d_camif *dcmi = (struct d_camif *)ci;

    dcmi_pdnrst_init();

    extclk_config(12000000);
    dcmi_config(dcmi, DCMI_JPEG_DISABLE);
    i2c_init(&dcmi->sccb);
    dma_config(dcmi);

    return 0;
}

static void d_dcmi_pwdn(rt_camif_t *ci, int level)
{
    if (level)
        DCMI_PWDN_HIGH();
    else
        DCMI_PWDN_LOW();
}

static void d_dcmi_reset(rt_camif_t *ci, int level)
{
    if (level)
        DCMI_RESET_HIGH();
    else
        DCMI_RESET_LOW();
}

static int d_reg_read(rt_camif_t *ci, uint8_t slv, uint16_t reg, int reglen, uint8_t *data)
{
    int ret = -1;
    struct d_camif *dcmi = (struct d_camif *)ci;

    if (reglen == 16)
        reglen = I2C_MEMADD_SIZE_16BIT;
    else
        ret = sccb_read8(dcmi, slv, (uint8_t)reg, data);

    return ret;
}

static int d_reg_write(rt_camif_t *ci, uint8_t slv, uint16_t reg, int reglen, uint8_t *data)
{
    int ret = -1;
    struct d_camif *dcmi = (struct d_camif *)ci;

    if (reglen == 16)
        reglen = I2C_MEMADD_SIZE_16BIT;
    else
        ret = sccb_write8(dcmi, slv, (uint8_t)reg, *data);

    return ret;
}

static int d_dcmi_ioctl(rt_camif_t *ci, int cmd, long args)
{
    int ret = -1;

    switch (cmd)
    {
    case VIDIOC_SET_XCLK:
    {
        extclk_config(args);
    }break;
    case VIDIOC_RESET:
    {
        if (args)
            DCMI_RESET_HIGH();
        else
            DCMI_RESET_LOW();
    }break;
    case VIDIOC_PWDN:
    {
        if (args)
            DCMI_PWDN_HIGH();
        else
            DCMI_PWDN_LOW();
    }break;
    }

    return ret;
}

static int d_dcmi_stream_start(rt_camif_t *ci)
{

    return 0;
}

static int d_dcmi_stream_stop(rt_camif_t *ci)
{

    return 0;
}

static int d_dcmi_set_format(rt_camif_t *ci, struct video_format *fmt)
{

    return 0;
}

static int d_dcmi_buf_active(rt_camif_t *ci, struct camif_buf *buf)
{
    struct d_camif *dcmi = (struct d_camif *)ci;

    HAL_DCMI_Start_DMA(&dcmi->DCMIHandle,
                        DCMI_MODE_SNAPSHOT, (uint32_t)buf->data, buf->bufsz/4);

    return 0;
}

static const struct camif_ops _ops =
{
    .init = d_dcmi_init,
    .pwdn = d_dcmi_pwdn,
    .reset = d_dcmi_reset,
    .sccb_read = d_reg_read,
    .sccb_write = d_reg_write,
    .ioctl = d_dcmi_ioctl,
    .stream_start = d_dcmi_stream_start,
    .stream_stop = d_dcmi_stream_stop,
    .set_format = d_dcmi_set_format,
    .buf_active = d_dcmi_buf_active,
};

int stm32_dcmi_init(void)
{
    _d_camif.ci.ops = &_ops;

    rt_camif_register(&_d_camif.ci, "camif0", &_d_camif);

    return 0;
}
INIT_BOARD_EXPORT(stm32_dcmi_init);
